﻿using System;
using System.Collections.Generic;
using System.Text;
using PositionSource.WebCam.Interfaces;

using RoverStudio.Math;

namespace CoordinateCalculator
{
    public class World3DFromPicture2D : ITransformationCalculator
    {
        double[,] intrinsicMatrix = new double[3, 3] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };

        double[,] distortionCoeffs = new double[1, 4] { { 0, 0, 0, 0 } };

        public World3DFromPicture2D()
        {
            //double fx = 949.1671;
            //double fy = 942.4894;
            //double cx = 319.5000;
            //double cy = 239.5000;

            double fx = 523.92621;
            double fy = 522.4602;
            double cx = 319.5000;
            double cy = 239.5000;

            intrinsicMatrix = new double[3, 3] { { fx, 0, cx }, { 0, fy, cy }, { 0, 0, 1 } };

//            distortionCoeffs = new double[1, 4] { { 0.1286, -0.8267, 0, 0 } };
            distortionCoeffs = new double[1, 4] { { -0.22266, 0.0767, 0.00325, 0.00556 } };
            
//            objectPoints = new double[8, 3] { { 0, 0, 0 }, { d, d, 0 }, { d, 0, 0 }, { d, -d, 0 }, { 0, -d, 0 }, { -d, -d, 0 }, { -d, 0, 0 }, { -d, d, 0 } };

            objectPoints = new double[8, 3] { { 0, 0, 0 }, { dx, dy, 0 }, { dx, 0, 0 }, { dx, -dy, 0 }, { 0, -dy, 0 }, { -dx, -dy, 0 }, { -dx, 0, 0 }, { -dx, dy, 0 } };

            //for (int i = 0; i < 8; i++)
            //{
            //    objectPoints[i,0] +=  0; // Central LED with respect to rover center
            //    objectPoints[i,1] += 70;
            //    objectPoints[i,2] += 130;
            //}
        }

        #region ITransformationCalculator Members

        public void SetFeatureModelCoordinates(Vector3D[] featureModelCoordinates)
        {
            throw new Exception("The method or operation is not implemented.");
        }

        public void SetCameraParameters(double[,] intrinsicMatrix, double[,] distortionCoeffs)
        {
            if (intrinsicMatrix == null || intrinsicMatrix.GetLength(0) != 3 || intrinsicMatrix.GetLength(1) != 3)
                throw new ArgumentException("Bad intrinsicMatrix parameter!");
            if (distortionCoeffs.GetLength(0)!=1 || distortionCoeffs.GetLength(1) != 4)
                throw new ArgumentException("Bad distortionCoeffs parameter!");
            this.intrinsicMatrix = intrinsicMatrix;
            this.distortionCoeffs = distortionCoeffs;
        }

        readonly double d = 2.54 * 18;

        readonly double dx = 2.54 * 18;
        readonly double dy = 2.54 * 19;
        double[,] objectPoints;

        public void CalculateTransformation(Vector2D[] featureImageCoordinates, out Vector3D position, out Rotation orientation)
        {

            double[,] imagePoints = new double[8, 2];

            for (int i = 0; i < featureImageCoordinates.Length; i++)
            {
                imagePoints[i, 0] = featureImageCoordinates[i].X;
                imagePoints[i, 1] = featureImageCoordinates[i].Y;
            }

            //double[] rotationVector;
            double[] translationVector;
            double[,] rotationMatrix;

    //        OpenVCWrapper.OpenCVWrapper.FindExtrinsicCameraParams(objectPoints, imagePoints,
    //intrinsicMatrix, distortionCoeffs, out translationVector, out rotationMatrix);
            OpenVCWrapper.OpenCVWrapper.FindExtrinsicCameraParams2(objectPoints, imagePoints,
                intrinsicMatrix, distortionCoeffs, out translationVector, out rotationMatrix);


            position = new Vector3D(translationVector[0], translationVector[1], translationVector[2]);
            position.Scale(0.001);
            orientation = new Rotation(rotationMatrix);

            Console.WriteLine("({0:00.00}, {1:00.00}, {2:00.00})", position.X, position.Y, position.Z);
            //orientation = new Rotation();
            
            //g.DrawString(string.Format("({0:00.00}, {1:00.00}, {2:00.00})", x / 1000, y / 1000, z / 1000), myFont, blueBrush, 0, 300);

            //double heading = 180 / Math.PI * Math.Atan2(rc[1, 1], rc[0, 1]);
            //				double tiltud = 180/Math.PI * Math.Atan2( rc[1,2], rc[2,2]);
            //				double tiltlr = 180/Math.PI * Math.Atan2( rc[0,2], rc[2,2]);
            //double tiltud = 180 / Math.PI * Math.Asin(rc[2, 1]);
            //double tiltlr = 180 / Math.PI * Math.Asin(rc[2, 0]);

            //g.DrawString(string.Format("(heading = {0:000.}°, tilt up/down = {1:00.00}, tilt left/right ={2:00.00})", heading, tiltud, tiltlr), myFont, blueBrush, 0, 350);

            //g.DrawString(string.Format("({0:00.00}, {1:00.00}, {2:00.00})", rc[0, 0], rc[0, 1], rc[0, 2]), myFont, blueBrush, 0, 400);
            //g.DrawString(string.Format("({0:00.00}, {1:00.00}, {2:00.00})", rc[1, 0], rc[1, 1], rc[1, 2]), myFont, blueBrush, 0, 420);
            //g.DrawString(string.Format("({0:00.00}, {1:00.00}, {2:00.00})", rc[2, 0], rc[2, 1], rc[2, 2]), myFont, blueBrush, 0, 440);
            //g.Dispose();
        }

        #endregion
    }
}
